%hw 7-2阻抗控制，这一题用回了7-1的末端误差控制

%%
simu_step = t_ed/simu_period;
add_step = simu_step;
simu_step = 3*simu_step;
%%
%注意数据存储方式，是组号(关节1/2)- 时间 - 数据类型（加速度速度或者位置）
state_info = zeros(2,length(1:simu_step),3);
debug_info = zeros(2,length(1:simu_step),1); %现在因为存的是力矩，就只有一列了
cart_info = zeros(2,length(1:simu_step),2);

%%
initial_state = [pi/3 0 0; -pi*2/3 0 0 ];
a_dj.reset_state(initial_state);
torque_info = zeros(2,length(1:simu_step),3);
joint_all_info = zeros(2,length(1:simu_step),3);


y = -0.01;
ky = 10;

%可以给两个关节指定不同的wn和zeta了
enable_m = repmat([1 1 0 0 0],2,1);
resist_con = doubleJointControler(a_dj,tor_motor,[0.5;0.5],[1;0.707],enable_m);
resist_con.pid_reset();
resist_con.resist_pid_update(10,2,0,-0.009,-0.01);

%%
%先把pid架设好
con_value =[0;0];
calculated_torque = [0;0];
sample_time_count = sample_period;



for i = 1:simu_step
    jacobi_m = a_dj.get_jacobi(0);
    cart_value = a_dj.get_cart_value(jacobi_m);
    cart_info(:,i,:) = cart_value;
    if(sample_time_count >= sample_period)
        resist_con.set_target([[step_value;y],zeros(2,3)]);
        if(cart_value(2,1) < 0)
            fe = [0; -cart_value(2,1)*ky];
        else
            fe = [0;0];
        end
        debug_info(:,i,:) = resist_con.get_resist_con_value(a_dj,cart_value,fe);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);
    end
    state_info(:,i,:) = (a_dj.simu_loop(con_value,simu_period));
    sample_time_count = sample_time_count+simu_period;

end


%% 末端xy展示
figure;
plot(0:simu_period:(simu_step-1)*simu_period,cart_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
yline(step_value,'--','Color','#EDB120');
legend('x','target');
title("x位移");


figure;
plot(0:simu_period:(simu_step-1)*simu_period,cart_info(2,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
yline(y,'--','Color','#EDB120');
legend('y','target');
title("y位移");


